

#include "drv8510.h"
#include "bsp_spi.h"
#include "math.h"

 struct KALMAN{
   float kg;
   float Q;
   float R;
   float x_pre;
   float p_pre;
   float p_last;
   float p_now;
   float x_now;
   float x_last;
   float x_pid;
}klm={0,0.01,3,0,0,0.03,0,0,10,0};

 uint8_t CNum=1;

 void delay_us(unsigned char a)
 {
    while(a--);
 }
 
 uint8_t SPI_READ(uint8_t Txdata)
{
    uint8_t rxdata;
	

    HAL_SPI_TransmitReceive(&bsp_spi.hspi1,&Txdata,&rxdata,1,200);

	
    return rxdata;
}
//---------------------------------------------------------
//函数介绍：Drv_As8510ReadReg读取寄存器函数
//参数说明：
//			uiReadAddr：寄存器地址
//
//返回值说明：
//			返回寄存器的值
//调用说明：
//			内部调用2022-11
//---------------------------------------------------------

 uint8_t Drv_As8510ReadReg(uint8_t uiReadAddr)
{ 
	uint8_t temp;
	uiReadAddr = 0x80|uiReadAddr;

	bsp_spi_lock();	
	HAL_GPIO_WritePin(GPIOB,GPIO_PIN_3,GPIO_PIN_RESET); // SPI片选有效 
	delay_us(20);
	SPI_READ(uiReadAddr);//此数据为要读的地址，
	temp=SPI_READ(uiReadAddr);//再读一次，这里数据不重要，才能读到数据。
	HAL_GPIO_WritePin(GPIOB,GPIO_PIN_3,GPIO_PIN_SET); // SPI片选有效
	bsp_spi_unlock();

	return temp;
}
//---------------------------------------------------------
//函数介绍：Drv_As8510WriteReg写寄存器函数
//参数说明：
//			uiWriteAddr：寄存器地址
//			ucValue:配置寄存器的值
//返回值说明：
//			void
//调用说明：
//			内部调用2022-11
//---------------------------------------------------------
void Drv_As8510WriteReg(uint8_t uiWriteAddr,uint8_t ucValue)
{   
	uint8_t temp[2];
	uint8_t rx;
	temp[0]=uiWriteAddr;
	temp[1]=ucValue;

	bsp_spi_lock();

	HAL_GPIO_WritePin(GPIOB,GPIO_PIN_3,GPIO_PIN_RESET);//片选
	delay_us(10);
	HAL_SPI_TransmitReceive(&bsp_spi.hspi1,temp,&rx,2,100);//设置寄存器的值
	delay_us(3);
	HAL_GPIO_WritePin(GPIOB,GPIO_PIN_3,GPIO_PIN_SET);

	bsp_spi_unlock();
}

//读取内部温度值
uint16_t ReadTemp()
{   
	uint8_t ucDreg_V1,ucDreg_V2;
    uint16_t rx;
    ucDreg_V1 = Drv_As8510ReadReg(DREG_V1);
    ucDreg_V2 = Drv_As8510ReadReg(DREG_V2);
    rx= (ucDreg_V1 << 8) | ucDreg_V2;
    return rx;
}

//电流增益设置
void Drv_CurrentGainSet(Gain_e Gain)
{
	uint16_t reg = 0;
	
	reg = Drv_As8510ReadReg(PD_CTL_REG_2);	
	
	if(Gain_1 == Gain)
	{
		reg |= 0x08;
		Drv_As8510WriteReg(PD_CTL_REG_2, reg);   //bypass PGA in voltage/Current Channel
	}
	else if(Gain_5 == Gain)
	{
		reg &= 0xF7;
		Drv_As8510WriteReg(PD_CTL_REG_2, reg);//旁路 电压通道，打开电流PGA通道
		Drv_As8510WriteReg(PGA_CTL_REG, 0x00);    //Current Gain 5
	}
	else if(Gain_25 == Gain)
	{
		reg &= 0xF7;
		Drv_As8510WriteReg(PD_CTL_REG_2, reg);
		Drv_As8510WriteReg(PGA_CTL_REG, 0x40);    //Current Gain 25
	}
	else if(Gain_40 == Gain)
	{
		reg &= 0xF7;		
		Drv_As8510WriteReg(PD_CTL_REG_2, reg);
		Drv_As8510WriteReg(PGA_CTL_REG, 0x80);    //Current Gain 40
	}
	else if(Gain_100 == Gain)
	{
		reg &= 0xF7;		
		Drv_As8510WriteReg(PD_CTL_REG_2, reg);
		Drv_As8510WriteReg(PGA_CTL_REG, 0xC0);    //Current Gain 100
	}
}


//电压增益设置
static void Drv_VoltageGainSet(Gain_e Gain)
{
	uint8_t ucmPgaCtrl=0;
	ucmPgaCtrl= Drv_As8510ReadReg(PGA_CTL_REG);
	
	if(Gain_1 == Gain)
	{
		Drv_As8510WriteReg(PD_CTL_REG_2,0xFF);   //bypass PGA in voltage/Current Channel
	}
	else if(Gain_5 == Gain)
	{
		Drv_As8510WriteReg(PD_CTL_REG_2,0xF3);
		//Drv_As8510WriteReg(PD_CTL_REG_2,0xFB);
		Drv_As8510WriteReg(PGA_CTL_REG, 0x00| (0xCF & ucmPgaCtrl));    //voltage Gain 5
//		Drv_As8510WriteReg(PGA_CTL_REG, 0x00);    //voltage Gain 5
	}
	else if(Gain_25 == Gain)
	{
		Drv_As8510WriteReg(PD_CTL_REG_2,0xF3);
		Drv_As8510WriteReg(PGA_CTL_REG, 0x10| (0xCF & ucmPgaCtrl));    //voltage Gain 25
	}
	else if(Gain_40 == Gain)
	{
		Drv_As8510WriteReg(PD_CTL_REG_2,0xF3);
		Drv_As8510WriteReg(PGA_CTL_REG, 0x20| (0xCF & ucmPgaCtrl));     //voltage Gain 40
	}
	else if(Gain_100 == Gain)
	{
		Drv_As8510WriteReg(PD_CTL_REG_2,0xF3);
		Drv_As8510WriteReg(PGA_CTL_REG, 0x30| (0xCF & ucmPgaCtrl));    //voltage Gain 100
	}
}



//---------------------------------------------------------
//函数介绍：Drv_As8510Init As8510芯片初始化配置函数
//参数说明：
//			void
//			
//返回值说明：
//			void
//调用说明：
//			内部调用
//完成时间：
//			2021.05.19
//
//修改时间：
//修改原因：
//---------------------------------------------------------
void Drv_As8510Init(void)
{
	uint8_t ucmPgaCtrl=0,ucmPdCtrl=0;	
	
	bsp_spi_init(&bsp_spi);
	
	
	Drv_As8510WriteReg(RESET_REG,0);	// Reset Chip	

    HAL_Delay(50);
	#if 1	
	//PGA-I5  PGA-V5
	Drv_As8510WriteReg(PGA_CTL_REG,0x00);	//Setting of Gain 5 G of voltage/Current Channel PGA
	Drv_As8510WriteReg(PD_CTL_REG_1,0xCF);
	Drv_As8510WriteReg(PD_CTL_REG_2,0xFF);// Bypass PGA in Current Channel
	//Drv_As8510WriteReg(PD_CTL_REG_2,0xF3);   //NO Bypass PGA in voltage/Current Channel
	Drv_As8510WriteReg(PD_CTL_REG_3,0xFC);   //Data Output in 2’s complement numbering system

	// CIC1-64, Chop-CLK=MOD_CLK/256 = 8KHz, CIC2-128  ,4KHz
	Drv_As8510WriteReg(DEC_REG_R1_I,0x45);//45
	Drv_As8510WriteReg(DEC_REG_R1_V,0x45);//45
	Drv_As8510WriteReg(DEC_REG_R2_I,0xC0); //fir/ma 输出

	Drv_As8510WriteReg(FIR_CTL_REG_I,0xfC); //滤波用最大值 5倍跳100个字 7c
//    Drv_As8510WriteReg(FIR_CTL_REG_I,0xfa); //                 2022-11-26
	Drv_As8510WriteReg(MOD_CTL_REG,0x06);   //启用了两个管脚的功能
	Drv_As8510WriteReg(CLK_REG,0x68);      //MEN active  HS 4MHz,MOD_CLK 1MHz
	//delay_ms(50);

//	ucmPgaCtrl= Drv_As8510ReadReg(PGA_CTL_REG);  //读取PGA状态
//	ucmPdCtrl = Drv_As8510ReadReg(PD_CTL_REG_2); //读掉电寄存器状态
	Drv_As8510WriteReg(PGA_CTL_REG, 0x05 | (0x3F & ucmPgaCtrl));    //Current Gain 5
//	Drv_As8510WriteReg(PD_CTL_REG_2,0x00 | (0xF7 & ucmPdCtrl));      //Enable current Gain
//	g_tAs8510Value.ucCurGain=100;

	//Enable Current source for ETS\ETR\internal temp sensor
	//Drv_As8510WriteReg(ACH_CTL_REG,0x10);  
	HAL_Delay(50);  
    
    Drv_As8510WriteReg(ACH_CTL_REG, GET_INT_TYPE);  //1101 0000 ,选中 内部温度通道   // Delay 50ms
    //Drv_VoltageGainSet(Gain_5);
	Drv_As8510WriteReg(ISC_CTL_REG,0); //内部电流设成0，其实未用到
	Drv_As8510WriteReg(MOD_CTL_REG,0x07); //从停止模式，到正常模式
	#endif
}



//读取电压，电流，ETS,ETR,内部温度值
//---------------------------------------------------------
//函数介绍：Drv_As8510ReadValue as8510读取数据函数
//参数说明：
//			*usCurrent：读取到的电流值
//			*usV2Value: 读取到的(电压,ETS,ETR,内部温度)ADC值
//			RegType:    类型选择
//返回值说明：
//			void
//调用说明：    2022-11

//---------------------------------------------------------
uint16_t Drv_As8510ReadCurrent(uint16_t *fCurrent)
{
	uint16_t usCurrent=0,usVValue=0,avgCurrent=0,avgValue=0;
	uint8_t uiDreg_I1=0,uiDreg_I2=0;
    uint8_t  uiDreg_V1=0,uiDreg_V2=0;
	uint8_t ucDreg_V1=0,ucDreg_V2=0,ucDreg_I1=0,ucDreg_I2=0;//用于保存都寄存器的值   
	static uint8_t Flag = 1; //作为是否切档标识位，为1时，没有切档，
//	const float A = -3.9658E-7,B = 2.0407E-5,C = 0.99974;//校准参数
	float Fcomp = 0.0f;
       SwitchCNum(CNum);    
//        ucDreg_V1 = Drv_As8510ReadReg(DREG_V1);
//		ucDreg_V2 = Drv_As8510ReadReg(DREG_V2);
//        
        for(uint8_t i=0;i<2;i++)
        {
            Drv_As8510ReadReg(DREG_I1);
		    Drv_As8510ReadReg(DREG_I2);
        }
                for(uint8_t i=0;i<2;i++)
        {
           avgCurrent += Drv_As8510ReadReg(DREG_I1);
		   avgValue   += Drv_As8510ReadReg(DREG_I2);
        }
        ucDreg_I1 = avgCurrent/2;
		ucDreg_I2 = avgValue/2;
        usCurrent = (ucDreg_I1 << 8) | ucDreg_I2;        
             	
		if( CNum ==1 && usCurrent < 5500) //1档，5倍，并且 电流值 太小
        { CNum +=1; if(CNum >4) CNum = 4; Flag = 0;} //增加一档，如果超最高，只给最高档
        
       else if( CNum ==2 && usCurrent < 16200) //2档，25倍，并且 电流值 太小
        { CNum +=1; if(CNum >4) CNum = 4;Flag = 0;} //增加一档，如果超最高，只给最高档
        
       else if( CNum ==3 && usCurrent < 9800) //3档，40倍，并且 电流值 太小
        { CNum +=1; if(CNum >4) CNum = 4;Flag = 0;} //增加一档，如果超最高，只给最高档       
 
        
      else if ( CNum >1 && usCurrent > 24074) //2,3,4档，如果值太大，减档
        {CNum -=1; if (CNum ==0) CNum =1;Flag = 0;}//降低一档       
        
        if(0==Flag) //如果切档了，重新读数
        {      SwitchCNum(CNum);              
            for(uint8_t i=0;i<2;i++)
            {
               Drv_As8510ReadReg(DREG_I1);
               Drv_As8510ReadReg(DREG_I2);
            }
            for(uint8_t i=0;i<6;i++)
            {
               avgCurrent += Drv_As8510ReadReg(DREG_I1);
               avgValue   += Drv_As8510ReadReg(DREG_I2);
            }
            ucDreg_I1 = avgCurrent/6;
            ucDreg_I2 = avgValue/6;
            usCurrent = (ucDreg_I1 << 8) | ucDreg_I2;   
            Flag =1 ; //复位  
            return usCurrent; 
        }   
        else
            return usCurrent;  
		//Log("ucDreg_I1 = %d,ucDreg_I2 = %d",ucDreg_I1,ucDreg_I2);
		/*for(i=0;i<10;i++)
		{
			uiDreg_I1 += Drv_As8510ReadReg(DREG_I1);
			uiDreg_I2 += Drv_As8510ReadReg(DREG_I2);
		}
		ucDreg_I1 = (u8)(uiDreg_I1/10);
		ucDreg_I2 = (u8)(uiDreg_I2/10);*/

	


//	Fcomp = A*(g_tAs8510Value.IntTemperature)*(g_tAs8510Value.IntTemperature) + B*(g_tAs8510Value.IntTemperature) + C;
//	//优化算法，减小执行时间
//	usV2Value = (s16)((ucDreg_V1 << 8) | ucDreg_V2);
	//usV2Value = (s16)(usV2Value*Fcomp);
		
//	usCurrent = (s16)((ucDreg_I1 << 8) | ucDreg_I2);

	//Log("usV2Value = %02d",(usV2Value));

//	if(usV2Value <= 32767)//电压通道正向
//	{
//		//*fV2Value = (float)((usV2Value*1.225)/32767);
//		//if((GET_INT_TYPE==RegType)||(GET_ETS_TYPE==RegType))
//		if(GET_INT_TYPE==RegType)
//		{
//			*fV2Value = (float)(usV2Value);
//		}
//		else
//		{
//			*fV2Value = (float)((usV2Value*1.225)/(32767));
//			//*fV2Value = (float)(usV2Value);
//		}
//	}
//	else//电压通道负向
//	{
//		//usV2Value =32767-(usV2Value & 0x7FFF);
//		//usV2Value =((~usV2Value) & 0x7FFF);
//		//usV2Value =65535-usV2Value;
//		//usV2Value =0;
//		*fV2Value = 0;
//	}
//	
//	if(usCurrent<= 32767)//电流通道正向
//	{
//		//*fCurrent = (float)((usCurrent*1.225)/32767);
//		*fCurrent = (float)(usCurrent);
//	}
//	else//电流通道负向
//	{
//		//usCurrent =((~usCurrent) & 0x7FFF);
//		usCurrent =65535-usCurrent;
//		//*fCurrent = (float)(-(usCurrent*1.225)/32767);
//		//*fCurrent = (float)(-usCurrent);
//		*fCurrent = 0;
//	}
//	//Log("usCurrent = %02d",(usCurrent));	
}

float Kal_Man(float res)//采集值

{    
   int temp=0;
    temp= fabs(res - klm.x_last);
    
    if (temp >500) 
        klm.Q = 0.4;
    else
        klm.Q = 0.01;
    klm.x_pre =klm.x_last;
   klm.p_pre = klm.p_last + klm.Q;
   klm.kg     = klm.p_pre / (klm.p_pre+ klm.R);
   klm.x_now = (1 - klm.kg) * klm.x_pre + klm.kg * res;
   klm.p_now = (1 - klm.kg) * klm.p_pre;
   klm.p_last = klm.p_now;
   klm.x_last = klm.x_now;
   return klm.x_now;
}


void SwitchCNum(uint8_t num)
{
   switch (num)//切档
       {
           case 1:   //5倍
              Drv_CurrentGainSet(Gain_5) ;
           break ;           
           case 2:   //25倍
               Drv_CurrentGainSet(Gain_25);
           break ;
           case 3:   //40倍
              Drv_CurrentGainSet(Gain_40) ;
           break ;
           case 4:   //100倍
               Drv_CurrentGainSet(Gain_100);
           break ;
           default :
               ;
           break;           
       } 
}
